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ABSTRACT 


Spatially random models provide an alternative to the more traditionsLdeterministic models used to 
describe robot arm dynamics. These alternative models can be used to^dtablish a relationship between 
the methodologies of estimation theory and robot dynamics. A new<£<»Wc:* of algorithms for many of the 
fundamental robotics problems of inverse and forward dynamics, inverse Kinematics, etc. can be developed 
that use computations typical in estimation theory. The algorithms make extensive use of the difference 
equations of Kalman filtering and Bryson- Fraaier smoothing to conduct spatial recursions. The spatially 
random models are very easy to describe and are based on the assumption that all of the inertial (D'Alembert) 
forces in the system are represented by a spatially distributed white-noise model. The models can also be used 
to generate numerically the composite multibody system inertia matrix. This is done without resorting to the 
more common methods of deterministic modeling involving Lagrangian dynamics, Newton-Euler equations, 
etc. These methods make substantiaKuse of human knowledge in derivation and manipulation of equations 
of motion for complex mechanical systems. In contrast, with the spatially random models, more primitive 
(i.e., simpler and less dependent on mathematical derivations) locally specified computations result in the 
emergence of a global collective system behavior equivalent to that obtained with the deterministic models. 

1. INTRODUCTION 

Recently, an equivalence has been discovered between estimation theory and recursive robot arm dy- 
namics [l], as summarized in the following table. 


TABLE 1 

Equivalence Between Optimal Estimation 
and 

Recursive Robot Arm Dynamics 

ROBOT DYNAMICS 
Spatial Forces 
Spatial Accelerations 
Joint Moments 
Spatial Jacobian 
Spatial Inertia Matrix 
Bias Spatial Force 
State-to-Joint-Axis Map 

A spatial force x(k) is a 6-dimensional vector consisting of three pure moment components and three 
force components. The argument k refers to a representative body k in a multibody system. Similarly, A(i) 
is a 6-dimensional vector of three angular acceleration components and three linear acceleration components. 
The joint moments r(i) are due to external sources acting at the joints. The spatial transition matrix serves 
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to propagate spatial forces within a body [1] in an inward direction from joint ft - 1 to joint ft. I U tran spose 
serves to propagate spatial accelerations in an opposite direction. The (V-by-6 matrix M(k) represents the 
spatial inertia of body ft about joint ft. The state- to-output map ff(ft) is a 1-by-fl vector that projects the 
spatial foreri'nCoi& component along the joint axis. The bias force ft(ft) is due to nonlinear velocity and 
gravity dependent effects [lj. 

The spatial inertia matrix and the transition matrix associated with this system are defined as 

**>■ -(_'&» T) 

*»>) 

in which /(ft) is the body ft inertia about joint ft; m(ft) is the body ft mass; L(k) is the vector from joint ft 
to joint ft - 1; and p(ft) is the vector from joint ft to the body ft mass center. The symbol U denotes the 
3-by-3 identity. 

A spatially random state space model for the multibody system is 

x‘(ft) = *(ft,ft-l)x+(ft -!) + «(*) ( U ) 


x + (k) = x“(ft) 0 2) 

in which x~(ft) is the value of the spatial force on the negative side of joint ft, and x+(ft - 1) is the value of 
the spatial force on the positive side of joint ft - 1. The u +" superscript indicates that the corresponding 
force is evaluated at a point immediately adjacent to joint ft and toward the base of the multibody system. 
Similarly, the superscript indicates that the corresponding variable is evaluated on the negative side of 
joint ft. Note th.it x+(ft - 1) and x~(ft) refer to spatial forces that are acting on body ft due to the adjacent 
bodies ft — 1 and ft + 1 respectively. Equation (1.2) expresses continuity of the spatial force in crossing a 

joint connecting two adjacent bodies. , . . 

The above is a linear model that reflects a balance of the forces that are acting on body ft. The inertial 
forces are represented by a spatial white-noise process whose mean and covariance are 

E[u(k)] = i(t) and E[Q(k)u{k) T ) ^ Af(fc) (1-3) 

with w(ft) = w(ft) - 6(fc). The mean value of the inertial force w(ft) is set equal to the bias force 6(i). 
The covariance of the inertial force is set equal to the spatial inertia matrix. The output, or measurement, 
equation 

r(fc) = J/(ft)x + (ft) (I- 4 ) 

completes a description of the stochastic model. In this model, the active joint moment r(ft) plays the 
role of the measurement in a linear state space system. Since the joint moments are known exactly, the 
corresponding measurement equation is free of measurement noise. 

The above model can be cast in the more compact notation 

X = <t>W and T = HX U- 5 ) 


where W, X , and T are the composite vectors W - Ml),... 
[r(l),...,r(N)J. Here, N represents the total number of bodies 
process error vector W has a mean and covariance given by 


,w(N)], * = [x(l), .... x(iV)l and T = 
in the multibody system. The composite 


E(W)-b and E[WW") = Q 


( 1 . 6 ) 
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where b = (6(1), ... , b(N)). The 6N-by-6N block diagonal matrix Q is defined as Q = diag[M (l), .... M (N)]. 
Its typical 6-by-6 diagonal block M(k) is the spatial inertia of body k. The matrix ^ is a causal (i.e M lower 
triangular) matrix defined as 

/ U 0 ... 0\ 

*(2,l) U ... 0 l 




(1.7) 


\<KNA) KN, 2) ... V/ 

The closely related composite state-to-output map H in (1-5) is defined as H =: d«aj[//(l), . . . , H ( Af )]. 

This model is now used to investigate a number of relationships between estimation theory and robot 
arm dynamics. 

2. CONDITIONAL MEAN ESTIMATION 


The estimation problem to be solved here is to estimate the process error vector W and the state X, 
given the measurements T. This corresponds to the dynamics problem of finding the inertial forces (due 
to accelerations) and the spatial forces, given the joint moments. The optimal estimates are obtained by 
means of the conditional expectations E{W/T) and E{X/T). It is relatively simple to compute these two 
conditional expectations, although care has to be exercised due to the non-zero mean of the inertial force 
W. By methods outlined in [2], it can be established that 


£(X/T) = ^ + C(T-//^) 


( 2 . 1 ) 


in which G is the “Kalman” gain 


G = RH*(HRH*)~ l and R = W (22) 

This is the estimate of the spatial forces given the applied joint moments. Note that the estimator equations 
have a predictor-corrector architecture. The prediction term is due to the bias force b in (2.1). This term 
“predicts” the cumulative spatial bias force on any given body due to the bias force acting on all of the 
preceding bodies. The covariance of the estimation error inherent in this “open-loop predicted estimate is 

£[(X - <t>b)(X - *6)-] = <t>Q<f = R (2-3) 

The prediction term is said to be open-loop because it is based only on the system model and doe* not 
depend on the measurement T. The effect of measurements is accounted for in the correction term involving 
the Kalman gain in (2.1). The Kalman gain determines the weight of the correction term, when this is added 
to the prediction term, to arrive at the final state estimate E(X/T). The Af-by-N matrix H RH that needs 
to be inverted to compute the Kalman gain turns out to be the composite muitibody system inertia matrix. 
To compute the covariance of the estimation error after correction has occurred, observe first that 

X - E(X/T) = (/ - GH)<(> W (2.4) 

is the estimation error. Its corresponding covariance is 

P - (I - GH)R(1 - GH)“ (2-5) 


Alternatively, this becomes 

P = (/ - GH)R — R(l - GH)* ~ R- RH*(H RH m )~ x HR (2.6) 

Note that HP = 0, PH* = 0 ,HPH* - 0 which imply that the estimation error at the joints vanishes. This 
reflects the lack of measurement noise in the measurement Equation (1.4). 

The conditional- mean estimate for the inertial forces is given by 

E(WjT ) =b+Q4*H*{HRH*)- l {T - H<t>b) (2.7) 
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This estimate is made up of two elements. First is the element due to the bias force b. Second is the element 
due to the active moments T. To examine these two effects more closely, define the jobt angle accelerations 


o = Af-‘(T- H<j>b) where M m HRH* (2*) 

Observe that the matrix M, whose inversion is required to compute the joint angle accelerations, is the 
composite multibody system inertia matrix. In addition, observe [1] that the jobt angle accelerations s and 
the spatial accelerations A are related by 

A = +*H*a (2.9) 

Based on these definitions, the estimate for the bertial forces becomes 

E(W/T) = b+Q\ (2.10) 

The covariance of the inertial force estimation error is obtained by arguments very similar to those used 
to arrive at (2.4). Observe first that W = W - E(W/T) = [/ - Q4 m H m (HRH*)“ l H4]W is the estimation 
error. Its covariance is 

E[WW] = Q-QfH*M~ l H4>Q (2.11) 

The foregoing are “batch” solutions to the estimation problem, in the sense that all of the measurements 
are processed simultaneously. This implies that the composite system bertia matrix must be inverted in a 
batch mode. An alternative is provided by the sequential solution outlined below. 

3. SEQUENTIAL ESTIMATION 

The sequential solution processes the measurements (the applied moments) one at a time. In doing this, 
it does not require numerical inversion of the N-by-N system bertia matrix. Instead, the bertia matrix is 
factored as 

Af = (/ + /0D(/ + /O (3.1) 

in which D is an N-by-N diagonal matrix, and K is a lower-triangular matrix. The matrices K and D in this 
factorization are generated using a suitably defined Kalman filter. This factorization of a covariance bto a 
product of a causal factor, a diagonal matrix, and the anti-causal adjoint factor is strongly reminiscent of 
the celebrated [5] Gohberg-Kreb factorization. Applications of this result to estimation problems have been 
investigated by Kailath [6]. Once this factorization of the system inertia matrix is achieved, the corresponding 
inverse can be computed easily. The central result is that 

(/ + /O' 1 - 1 ~ L (3-2) 

where L is a lower- triangular causal matrix generated by the same Kalman filter that generates K. This 
implies that the inertia matrix inverse can be expressed as 

At -1 = (/ — L m )D~ l (l - L) (3.3) 

The central aim of this section is to outline how to obtain this result. Only the major results are 
presented. The detailed arguments leading to the results will be presented elsewhere by the author. 

Result 3.1. The state covariance matrix R = can be expressed as 

R = r + <&r + rO* (3.4) 


Here, is the system model matrix obtained by subtractbg the 6Af-by -6N identity from the matrix in 
(1.7). The matrix r is a 6tf-by-6N block diagonal matrix r = dio^(r(l), . . . ,r(Af)] whose blocks r{k) satisfy 
the recursive relationships 

r + (0) = 0 


r-(*) = 4>{k,k - 1 )r + (fc - l)4 T (k,k — 1) 4- M(k) (3.5) 

r+(*) = r"(*) 

Define now the block-diagonal matrix P = dia$[P(l), . . . , P(tf)] whose diagonal blocks P(i) satisfy the 


discrete Riccati equation 

P~(k) = <j>(k,k- 1 )P + (* - 1 )* T (M - 1) + A/(fc) 

D{k) = H(k)P~(k)H T (k) (3-6) 

P+(k) = P“(i)- P-(k)H T (k)H(k)p-(k)/D(k) 


with the “initial” condition P+ (0) = 0. 
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This also implies that K — L + KL, K ~ L + LK, and LK — KL. 

The above sequence of results is the necessary ingredient to establish the recursive factorization of 
the inverse of the composite system inertia matrix as in (3.3). 

4. FILTERING AND SMOOTHING 

T'ypicaHy. the composite system inertia matrix is inverted to solve what is referred to as the forward 
dynamics problem. This problem consists of computing a set of joint angle accelerations given a corresponding 
set of applied joint moments. The joint angle accelerations a and the applied joint moments T are related 
by 

a = (I - L*)D~ l (I - L)T (4.1) 

where a = [a(l), . . . , a(JV)] is the vector of joint angle accelerations. This states that the joint moments must 
be processed by means of a two-stage computation. The first stage represents filtering and is characterized 
by the factor (/ - L). 

The second stage represents smoothing and is characterized by the factor (/ — !>*). 
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Filtering . This stage produces an “innovations” process defined as 


e~=(7- L)T (4.2) 

It produces also the filtered state estimate 

Z = *PH*D~ l T = (4.3) 

The components z(lr) of Z = (z(l), . . . , z(JV)] satisfy the Kalman filter equations (1] 

z’(fc) = ^(*, k - l)ar+(* - 1) + 6(fc) (4- 4 ) 

: + (t) = r(i) + ff(i)«’(i) (4.5) 

in which e”(fc) are the elements of the innovations vector e” = [e”(l) f ...,e“(iV)]. Multiplication of the 
innovations process by the inverse of the diagonal matrix D produces the residuals 

c+ =D~ l e~ (4.6) 


These residuals are processed in the smoothing stage that follows. 

Smoothing. This corresponds to multiplication of the residuals by the anti-causal factor (/ — L *) to 
obtain the joint angle accelerations, i.e., 

a = (/ - L*)e + (4.7) 

A spatial difference equation which is based on (4.7) can be obtained by re-introducing the co-state variables 
defined earlier. The co-state variables A and the residuals e + are related by 

A = ¥*/Te + (4.8) 

Use of this in (4.7) implies that 

a = e+-**A (4.9) 

This last relationship expresses the joint angle accelerations in terms of the residuals and the co-state 
variables. Furthermore, (4.8) can be used to infer that the co-state variables satisfy the difference equation 

A + (* - 1) = <f> T (k,k - 1)A ~(k) (4.10) 

A”(i) = A +(*) + ff T (k)e+( Jfc) (4.11) 

with the terminal condition A + (iV) = 0. These equations are referred to as the Bryson-Frazier smoother 
equations [4]. Their application to problems in robot dynamics is discussed in more detail in [1]. 

5. COVARIANCE ANALYSIS 

The aim here is develop formulas to compute the covariance of several relevant quantities (state, state 
estimation error, innovations, etc.) discussed in previous sections. The stochastic model (1.5) is assumed as 
a starting point. As in earlier discussions, the results are stated without proof. 

Result 5.1. The composite system inertia matrix M is the covariance of the measurement process, i.e., 

M = E(TT*) = HRIT (5.1) 

This result has an interesting interpretation. It states that the collective system behavior, as represented 
by the system inertia, emerges from the covariance of the output T of the spatially random model (1.5). It 
therefore provides a means to compute the in /tia matrix numerically by direct simulation of the stochastic 
model. From such a simulation, the inertia matrix would emerge (without conducting the more traditional 
manual derivation of the equations of motion). 
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Result 5 . 2 . The spatial inertia matrix P produced by the Riccati equation is equal to the covariance of 
the state estimation error, i.e., 

E[(X - Z)(X - Z)* ] m P + «P + P’9* (5.2) 

The corresponding mean-square estimation error is 

E[(X - ZY(X - Z)] = Tr(Pj (5.3) 

Result 5 . 3 . The innovations process has a covariance given by 

£[(«-)(«*)'] = D (5.4) 

Result 5 . 4 . The covariance of the co-states is 

E(AV) = 9'ff*D- 1 ff9 = A + A¥ + **A (5.5) 

in which A = diag (A(l), . . . , A(JV)]. The diagonal blocks A (k) satisfy 

A -(*) = [/ - j(i)//(Jt)] T A+(t)[/ - g(h)II(k)) + H T (k)H(k)/D(k) (5.6) 

A+(* - 1) = * T (t,t - 1)A -(k)t(k,k- 1) (5.7) 

with the terminal condition A + (N) = 0. 

0. CLOSED-FORM INERTIA MATRIX INVERSE 
The foregoing results can be used to obtain in closed form the inverse of the composite multibody system 
inertia. This is done in terms of the covariance matrices P and A of the previous section. 

Result 6.1. The inverse of the system inertia matrix can be expressed as 

M~ l = D~ l + g'Ag + * *(A(/ - H*D~ X ) + (g * A - Xr l /f)#<7 (6.1) 

Alternatively, it can be expressed as M ~ 1 = D~ l HSH* D~ l where 

S — P + PAP + P¥*(AP - /) + (PA - I)*P (6.2) 

This result is quite similar to that obtained in [1] by more detailed methods. The result has an interesting 
potential application in robot dynamics analysis and in control design. The equations of motion for the 
multibody system representing a robot arm are typically written, neglecting bias forces and accelerations 
due to nonlinear velocity and gravity dependent effects, in the form 

Ma = T (6.3) 

where a is the set of joint angle accelerations, and T is the set of applied joint moments. The primary 
reason for the widespread use of such an equation is that many of the known methods for deriving equations 
of motion result in a matrix equation of this form. The equation consistently involves the presence of a 
composite system inertia matrix. There is, however, nothing intrinsic in the multibody dynamics problem 
that would make the presence of an inertia matrix in the equations of motion completely inevitable. In 
fact, Result 6.1 shows how to compute the inverse of the inertia matrix directly, without having to evaluate 
the inertia matrix first and then having to invert it. It is therefore possible, by using this result, to arrive 
directly, without numerical inversion of the inertia matrix, at a set of motion equations of the form 

a — M~ l T (6.4) 

This is potentially a very useful result, since the system in (6.4) is much easier to work with, in simulation 
and control design, for instance, than the equivalent system in (6.3). 
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7. CONCLUDING REMARKS 

The use of spatially distributed random models has been explored in analysing robot arm dynamics. 
Based on such models a previously undiscovered relationship has been established between estimation theory 
and recursive robot arm dynamics. Many of the fundamental problems in robot dynamics can be approached 
using the techniques of estimation theory. The interaction between these two areas has not been recognized 
before and leads to many useful insights, such as the equivalence of covariance and spatial inertia. The 
numerical properties of the new algorithms emerging from the estimation approach to robot dynamics are 
under investigation. 
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